#include <string.h>

#include "DAP_global.h"

volatile uint8_t  USB_RequestFlag;       // Request  Buffer Usage Flag
volatile uint32_t USB_RequestIn;         // Request  Buffer In  Index
volatile uint32_t USB_RequestOut;        // Request  Buffer Out Index

volatile uint8_t  USB_ResponseIdle;      // Response Buffer Idle  Flag
volatile uint8_t  USB_ResponseFlag;      // Response Buffer Usage Flag
volatile uint32_t USB_ResponseIn;        // Response Buffer In  Index
volatile uint32_t USB_ResponseOut;       // Response Buffer Out Index

uint8_t  USB_Request [DAP_PACKET_COUNT][DAP_PACKET_SIZE];  // Request  Buffer
uint8_t  USB_Response[DAP_PACKET_COUNT][DAP_PACKET_SIZE];  // Response Buffer

#define PACK_DATA_PLONG(offs)	(uint32_t *)(*(uint32_t *)(request + offs))
#define PACK_DATA_PBYTE(offs)	(uint8_t *) (*(uint32_t *)(request + offs))
#define PACK_DATA_LONG(offs)	*(uint32_t *)(request + offs)
#define PACK_DATA_WORD(offs)	*(uint16_t *)(request + 4)

#define HID_Command0	0xD0
#define HID_Command1	0xD1
#define HID_Command2	0xD2
#define HID_Command3	0xD3
#define HID_Command4	0xD4
#define HID_Command5	0xD5
#define HID_Command6	0xD6
#define HID_Command7	0xD7

void HID_ProcessCommand(uint8_t *request, uint8_t *response)
{
    uint8_t result   = 0xFF;  //! DAP_OK;
    uint16_t data;
    uint16_t length;
    uint32_t address;
    uint8_t *p_address;

    if ((*request >= HID_Command0) && (*request <= HID_Command7))
    {
        DEBUG("REQ:%2X\n", *request);

        *response++ = *request;
        switch (*request++)
        {

        // Get device info
        case HID_Command0:
            address = DBGMCU->IDCODE;
            *response++ = (address >>  0) & 0xFF;
            *response++ = (address >>  8) & 0xFF;
            *response++ = (address >> 16) & 0xFF;
            *response++ = (address >> 24) & 0xFF;
            break;

        // Unlock flash
        case HID_Command1:
//			FLASH_Unlock();
            break;

        // Lock flash
        case HID_Command2:
//			FLASH_Lock();
            break;

        // Erase page
        case HID_Command3:
            address = PACK_DATA_LONG(0);
//			if (FLASH_ErasePage(address) != FLASH_COMPLETE)
//				result = DAP_ERROR;
            break;

        // Check for blank (0xFF)
        case HID_Command4:
            p_address = PACK_DATA_PBYTE(0);
            length  = PACK_DATA_WORD(4);
            while (length-- != 0)
                if (*p_address++ != 0xFF)
                {
                    result = DAP_ERROR;
                    break;
                }
            break;

        // Write to flash
        case HID_Command5:
            address = PACK_DATA_LONG(0);
            length  = PACK_DATA_WORD(4);
            if (length > (DAP_PACKET_SIZE - (6+2)))	// Check for maximum data payload
                result = DAP_ERROR;
            else
            {
                request += 6;
                while (length-- != 0)
                {
                    data = *request++;
                    if (length != 0)
                    {
                        data |= ((uint16_t)(*request++) << 8);
                        length--;
                    }
//					if (FLASH_ProgramHalfWord(address, data) != FLASH_COMPLETE)
//					{
//						result = DAP_ERROR;
//						break;
//					}
                    address += 2;
                }
            }
            break;

        // Read from flash
        case HID_Command6:
        {
            p_address = PACK_DATA_PBYTE(0);
            length  = PACK_DATA_WORD(4);
            if (length > (DAP_PACKET_SIZE - (2)))	// Check for maximum data payload
                result = DAP_ERROR;
            else
            {
                *response++ = DAP_OK;
                while (length-- != 0)
                {
                    if (length == 0)
                        result = *p_address;
                    else
                        *response++ = *p_address++;
                }
            }
        }
        break;

        // Reset device
        case HID_Command7:
            NVIC_SystemReset();
            break;

        default:
            --response;
            result = ID_DAP_Invalid;
        }
        *response = result;
    }
    else
    {
        if (pUserAppDescriptor != NULL)
        {
            pUserAppDescriptor->UserProcess(USB_Request[USB_RequestOut], USB_Response[USB_ResponseIn]);
        }
        else
        {
            DEBUG("REQ:%02X no app\n", *request);
            *response = ID_DAP_Invalid;
        }
    }

    DEBUG("RES:%2X\n", *response);
}


// Process USB HID Data
uint8_t MY_CMSIS_DAP_Process (void)
{
    uint32_t n;

    // Process pending requests
    if ((USB_RequestOut != USB_RequestIn) || USB_RequestFlag)
    {
        HID_ProcessCommand(USB_Request[USB_RequestOut], USB_Response[USB_ResponseIn]);

        // Update request index and flag
        n = USB_RequestOut + 1;
        if (n == DAP_PACKET_COUNT)
            n = 0;
        USB_RequestOut = n;

        if (USB_RequestOut == USB_RequestIn)
            USB_RequestFlag = 0;


        /*-------------------------------*/

        n = USB_ResponseIn + 1;
        if (n == DAP_PACKET_COUNT)
            n = 0;
        USB_ResponseIn = n;

        if (USB_ResponseIn == USB_ResponseOut)
            USB_ResponseFlag = 1;

        return 1;
    }
    return 0;
}
